//
// Created by 16933 on 2022/5/2.
//

#include "Encoder.h"
#include "tim.h"
void encoder_data_handle(motor_measure_t *ptr ,int16_t data)
{
    ptr->temp_sum = 0;
    ptr->last_ecd      = ptr->ecd;
    ptr->ecd = data;
    //过零点判断 如果两次取值之间相差的比每一圈输出的脉冲的一半还多，那么认为是反向了过零点了
    if (ptr->ecd - ptr->last_ecd > (PULSE_ONE_LAP/2))
    {
        ptr->round_cnt--;
        ptr->ecd_raw_rate = ptr->ecd - ptr->last_ecd - PULSE_ONE_LAP;
    }
    else if (ptr->ecd - ptr->last_ecd < -(PULSE_ONE_LAP/2))
    {
        ptr->round_cnt++;
        ptr->ecd_raw_rate = ptr->ecd - ptr->last_ecd + PULSE_ONE_LAP;
    }
    else
    {
        ptr->ecd_raw_rate = ptr->ecd - ptr->last_ecd;
    }
    ptr->total_ecd_last = ptr->total_ecd;

    ptr->total_ecd = ptr->round_cnt * PULSE_ONE_LAP + ptr->ecd_raw_rate;

    ptr->total_ecd_delta = ptr->total_ecd-ptr->total_ecd_last;

    ptr->total_angle = ptr->total_ecd * 360 / PULSE_ONE_LAP;

//    ptr->speed_rpm = (((ptr->ecd_raw_rate/DELAT_T)))*10.0;        //减小对噪声的放大因此不取正经的rpm
    ptr->speed_rpm = (float)((((ptr->total_ecd_delta)/DELAT_T)*1000)/PULSE_ONE_LAP)*60/6;  //每10m转多少圈

//    ptr->speed_rpm = ((float)(ptr->total_ecd_delta)/DELAT_T);
    if (ptr->speed_rpm>SPEED_MAX)
    {
        ptr->speed_rpm = SPEED_MAX;
    }
    if(ptr->speed_rpm<-SPEED_MAX)
    {
        ptr->speed_rpm = -SPEED_MAX;
    }

//            (float)(((float)ptr->ecd_raw_rate/(float)(PULSE_ONE_LAP*DELAT_T))*1000*60);
}


/*** 自定义函数 ***/
void encoder_reset(motor_measure_t *ptr)
{
    ptr->ecd = 0;
    ptr->last_ecd = 0;
    ptr->speed_rpm = 0;
    ptr->ecd_raw_rate = 0;
    ptr->round_cnt = 0;
    ptr->total_ecd = 0;
    ptr->total_angle = 0;
    ptr->buf_cut = 0;
    ptr->temp_sum = 0;
}
int encoder_filter(int encoder_left,int encoder_right)
{
    static float Velocity=0,Encoder_Least=0,Encoder=0,Movement=0;
    static float Encoder_Integral=0;
    Encoder_Least = (encoder_left+encoder_right)-0;
    Encoder *= 0.8;
    Encoder += Encoder_Least*0.2;
    return Encoder;
}

void encoder_speed_handle(motor_measure_t *ptr ,int16_t data)
{
    static uint8_t i = 0;
    ptr->ecd = data - 100;
    static float sum = 0;
    if (i < CAL_TIME)
    {
        sum += ptr->ecd;
        i++;
    }
    else
    {
        sum /= CAL_TIME*DELAT_T;
        ptr->speed = sum;
        sum = 0;
        i = 0;
    }
    __HAL_TIM_SET_COUNTER(&htim2,100);
    if (ptr->speed > SPEED_MAX)
    {
        ptr->speed = SPEED_MAX;
    }
    if(ptr->speed<-SPEED_MAX)
    {
        ptr->speed = -SPEED_MAX;
    }

}